#pragma once

#include <math.h>
#include <assert.h>

/** 
* Basic definitions used in 3D math
*/
namespace PBVP {
	// 
	typedef float Radian;
	typedef float Real;

	class Math {
	public:
		static const float POS_INFINITY;
		static const float NEG_INFINITY;
		static const float PI;
		static const float TWO_PI;
		static const float HALF_PI;

		static bool floatEqual(double x, double y, double epsilon)
		{
			return fabs(x - y) <= epsilon;
		}

		/** Inverse square root i.e. 1 / Sqrt(x), good for vector
		normalisation.
		*/
		static float InvSqrt(float x) {
			return 1/sqrt(x);
		}

		static float Sqr(float x) {
			return x * x;
		}

		static float ATan2(float y, float x) {
			return atan2(y, x);
		}

		static float Sin(float x) {
			return sin(x);
		}
		static float Cos(float x) {
			return cos(x);
		}
		static float ACos(float x) {
			return acos(x);
		}
		static float ASin(float x) {
			return asin(x);
		}

		static float Abs(float x) {
			return fabsf(x);
		}

		static unsigned int toPowerOfTwo(unsigned int n) {
			unsigned int c = 0;
			unsigned int v = n;

			// round down to nearest power of two
			while (v>1) {
				v = v>>1;
				c++;
			}

			// if that isn't exactly the original value
			if ((v<<c) != n) {
				// return the next power of two
				return(v << (c+1));
			} else {
				// return this power of two
				return(v << c);
			}
		}

		static float normalize3f(float n[3])
		{
			float  result;
			result = (float) sqrt(n[0]*n[0] + n[1]*n[1] + n[2]*n[2]);
			if (result == 0.0) {
				/* default vector */
				n[0] = n[1] = 0; n[2] = 1;
				return 0;
			}
			n[0] = n[0]/result;
			n[1] = n[1]/result;
			n[2] = n[2]/result;

			return result;
		}

		static int log2(int n) {
			assert(n > 0);
			int k = 0;
			while (n > 1) {
				n >>= 1 ;
				k++;
			}
			return k;
		}

		static int clamp(int n, int min, int max)
		{
			int m = n;
			if (m < min) {
				m = min;
			} else if (m > max) {
				m = max;
			}
			return m;
		}

		static float Clampf(float _x, float min, float max)
		{
			float x = _x;
			if (x < min) {
				x = min;
			} else if (x > max) {
				x = max;
			}
			return x;
		}
	};
}